copp\copp\copp3/formulation.rs
1//! Problem data models and builders for TOPP3/COPP3.
2//!
3//! # Notation policy (math + code)
4//! To help users map paper notation to API fields without ambiguity,
5//! this module follows a dual notation style:
6//! - mathematical definition uses KaTeX, e.g. $a_k = \dot{s}_k^2$ and $b_k = \ddot{s}_k$;
7//! - discrete implementation uses code symbols, e.g. `a[k]`, `b[k]`, `s[k]`.
8//!
9//! # Reference
10//! Wang, Y., Hu, C., Li, Y., Yu, J., Yan, J., Liang, Y., & Jin, Z. (2026).
11//! Online time-optimal trajectory planning along parametric toolpaths with strict constraint
12//! satisfaction and certifiable feasibility guarantee.
13//! *International Journal of Machine Tools and Manufacture*, 215, 104355.
14//! <https://doi.org/10.1016/j.ijmachtools.2025.104355>
15//!
16//! # Stationary-boundary modeling note
17//! This module exposes `num_stationary_max=(start,end)` on builders as a **user input upper
18//! bound** and derives effective `num_stationary` during `build_with_linearization()`.
19//! For typical users, `num_stationary_max=(1,1)` is recommended.
20//!
21//! For stationary boundaries (`a=b=0`), using zero stationary intervals is allowed, but the
22//! boundary-time model can become ill-conditioned because the regular
23//! $c=\frac{\dddot{s}}{\dot{s}}$-constant formulation degenerates near zero speed.
24//! A short boundary neighborhood modeled by $\dddot{s}$-constant stationary intervals is the
25//! practical remedy.
26
27use crate::copp::CoppObjective;
28use crate::copp::constraints::Constraints;
29use crate::diag::{CoppError, check_boundary_state_copp3_valid, check_s_interval_valid};
30use crate::robot::robot_core::{Robot, RobotBasic, RobotTorque};
31use itertools::izip;
32
33const DEFAULT_A_LINEARIZATION_FLOOR: f64 = 1E-10;
34const DEFAULT_NUM_STATIONARY_MAX: (usize, usize) = (1, 1);
35
36/// Borrowed third-order TOPP/COPP profile parts.
37///
38/// This view keeps APIs lightweight for callers that already own separate
39/// `a`/`b` slices while preserving the same semantic grouping as
40/// [`Topp3Profile`]. The tuple layout is `(a, b, num_stationary)`.
41pub type Topp3ProfileRef<'a> = (&'a [f64], &'a [f64], (usize, usize));
42
43/// Mutable borrowed third-order TOPP/COPP profile parts.
44///
45/// This view is intended for in-place profile post-processing. The tuple
46/// layout is `(a, b, num_stationary)`, where only the `a` and `b` slices are
47/// mutable and the stationary boundary counts remain immutable metadata.
48pub type Topp3ProfileMut<'a> = (&'a mut [f64], &'a mut [f64], (usize, usize));
49
50/// Node-based third-order TOPP/COPP profile.
51///
52/// The three fields are tied together by convention: `a.len() == b.len()`,
53/// and `num_stationary` describes stationary head/tail intervals on the same
54/// path grid used with those node profiles.
55#[derive(Clone, Debug, PartialEq)]
56pub struct Topp3Profile {
57 /// Node samples of $a_k = \dot{s}_k^2$.
58 pub a: Vec<f64>,
59 /// Node samples of $b_k = \ddot{s}_k$.
60 pub b: Vec<f64>,
61 /// Stationary boundary interval counts as `(head, tail)`.
62 ///
63 /// `head` counts stationary intervals at the start of the grid, and
64 /// `tail` counts stationary intervals at the end. These markers are used
65 /// by TOPP3/COPP3 interpolation and timing reconstruction.
66 pub num_stationary: (usize, usize),
67}
68
69impl Topp3Profile {
70 /// Build a third-order profile from owned parts.
71 #[inline]
72 pub fn new(a: Vec<f64>, b: Vec<f64>, num_stationary: (usize, usize)) -> Self {
73 Self {
74 a,
75 b,
76 num_stationary,
77 }
78 }
79
80 /// Borrow the profile as `(a, b, num_stationary)` parts.
81 #[inline]
82 pub fn as_parts(&self) -> Topp3ProfileRef<'_> {
83 (&self.a, &self.b, self.num_stationary)
84 }
85
86 /// Mutably borrow the profile as `(a, b, num_stationary)` parts.
87 #[inline]
88 pub fn as_parts_mut(&mut self) -> Topp3ProfileMut<'_> {
89 (&mut self.a, &mut self.b, self.num_stationary)
90 }
91
92 /// Consume the profile and return `(a, b, num_stationary)`.
93 #[inline]
94 pub fn into_parts(self) -> (Vec<f64>, Vec<f64>, (usize, usize)) {
95 (self.a, self.b, self.num_stationary)
96 }
97}
98
99#[inline(always)]
100fn determine_num_stationary_side(a: f64, b: f64, num_stationary_max: usize) -> usize {
101 if a.abs() < f64::EPSILON && b.abs() < f64::EPSILON {
102 num_stationary_max
103 } else {
104 0
105 }
106}
107
108#[inline(always)]
109fn determine_num_stationary_pair(
110 a_boundary: (f64, f64),
111 b_boundary: (f64, f64),
112 num_stationary_max: (usize, usize),
113) -> (usize, usize) {
114 (
115 determine_num_stationary_side(a_boundary.0, b_boundary.0, num_stationary_max.0),
116 determine_num_stationary_side(a_boundary.1, b_boundary.1, num_stationary_max.1),
117 )
118}
119
120/// Prepared TOPP3 problem view.
121///
122/// # Method identity
123/// This is the **read-only runtime view** consumed by TOPP3/COPP3 solvers after
124/// third-order constraints have been linearized.
125///
126/// # Important invariant
127/// The builder precomputes linearized jerk buffers in [`Constraints`](crate::constraints::Constraints):
128/// - `jerk_a_linear`
129/// - `jerk_max_linear`
130///
131/// and this object subsequently holds only `&Constraints` (non-mutable view).
132///
133/// # Why this works
134/// Original third-order inequality includes a nonlinear denominator term:
135/// $$
136/// \sqrt{a(s)}\left(g\_a(s) a(s) + g\_b(s) b(s) + g\_c(s) c(s) + g\_d(s)\right) \le g\_{\text{max}}(s).
137/// $$
138///
139/// Around reference `a_linearization`, it is approximated into affine form:
140/// $$
141/// \left(g\_a(s) + \frac{g\_{\text{max}}(s)}{2a_{\text{lin}}^{3/2}}\right)a + g\_b(s) b + g\_c(s) c
142/// \le \frac{3g\_{\text{max}}(s)}{2a_{\text{lin}}^{1/2}} - g\_d(s).
143/// $$
144/// where $a_{\text{lin}}$ corresponds to code input `a_linearization[k]`.
145///
146/// The affine coefficients are stored into those two buffers for downstream LP/SOCP/RA use.
147pub struct Topp3Problem<'a> {
148 pub(crate) constraints: &'a Constraints,
149 pub(crate) idx_s_start: usize,
150 pub(crate) a_linearization: &'a [f64],
151 pub(crate) a_boundary: (f64, f64),
152 pub(crate) b_boundary: (f64, f64),
153 /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
154 /// boundary conditions and `num_stationary_max`.
155 pub(crate) num_stationary: (usize, usize),
156}
157
158/// Builder for [`Topp3Problem`](crate::solver::topp3_socp::Topp3Problem), including optional in-build linearization.
159///
160/// # Side effect notice
161/// [`build_with_linearization`](Topp3ProblemBuilder::build_with_linearization) updates
162/// cached affine linearization data inside [`Constraints`](crate::constraints::Constraints).
163/// Raw jerk constraints remain unchanged.
164///
165/// # Why mutable?
166/// Third-order jerk rows are stored in nonconvex form after
167/// [`Robot::with_axial_jerk`](crate::robot::Robot::with_axial_jerk) or
168/// [`Constraints::with_constraint_3order`](crate::constraints::Constraints::with_constraint_3order).
169/// [`build_with_linearization`](Topp3ProblemBuilder::build_with_linearization)
170/// evaluates an affine approximation around
171/// [`a_linearization`](Topp3ProblemBuilder::a_linearization) and
172/// writes the generated coefficients into the constraint
173/// cache. Rebuild the problem whenever the linearization profile changes.
174///
175/// # Example
176/// The example below builds a TOPP3 problem and lets the builder linearize the third-order constraints.
177///
178/// ```rust
179/// # fn main() -> Result<(), copp::diag::CoppError> {
180/// use copp::robot::Robot;
181/// use copp::solver::topp3_lp::Topp3ProblemBuilder;
182///
183/// let mut robot = Robot::with_capacity(2usize, 3);
184/// let s = [0.0, 0.5, 1.0];
185/// robot.with_s(s.as_slice())?;
186///
187/// let a_linearization = [0.0, 0.25, 0.0];
188/// let _problem = Topp3ProblemBuilder::new(
189/// &mut robot,
190/// 0,
191/// &a_linearization,
192/// (0.0, 0.0),
193/// (0.0, 0.0),
194/// )
195/// .build_with_linearization()?;
196/// # Ok(())
197/// # }
198/// ```
199pub struct Topp3ProblemBuilder<'a> {
200 /// Mutable constraint storage used to build linearized TOPP3 problem data.
201 pub constraints: &'a mut Constraints,
202 /// Start station index of the optimization interval.
203 pub idx_s_start: usize,
204 /// Reference profile `a[k]` used to linearize third-order constraints.
205 pub a_linearization: &'a [f64],
206 /// Boundary values of `a=(a_start, a_final)`.
207 pub a_boundary: (f64, f64),
208 /// Boundary values of `b=(b_start, b_final)`.
209 pub b_boundary: (f64, f64),
210 /// User-input upper bound of stationary intervals at (start, end).
211 pub num_stationary_max: (usize, usize),
212 /// Denominator floor for stable evaluation of `1/sqrt(a_linearization)` near `a=0`.
213 ///
214 /// Effective usage in linearization is:
215 /// $$
216 /// \frac{1}{\sqrt{\max(a_{lin}, a_{floor})}}.
217 /// $$
218 /// Discrete code form:
219 /// `1.0 / max(a_linearization, a_linearization_floor).sqrt()`.
220 ///
221 /// More details are available in the [`Topp3Problem`](crate::solver::topp3_socp::Topp3Problem) documentation.
222 pub a_linearization_floor: f64,
223}
224
225impl<'a> Topp3ProblemBuilder<'a> {
226 /// Create a TOPP3 builder with required fields.
227 ///
228 /// Defaults:
229 /// - `num_stationary_max = (1, 1)`
230 /// - `a_linearization_floor = 1E-10`
231 pub fn new<M: RobotBasic>(
232 robot: &'a mut Robot<M>,
233 idx_s_start: usize,
234 a_linearization: &'a [f64],
235 a_boundary: (f64, f64),
236 b_boundary: (f64, f64),
237 ) -> Self {
238 Self {
239 constraints: &mut robot.constraints,
240 idx_s_start,
241 a_linearization,
242 a_boundary,
243 b_boundary,
244 num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
245 a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
246 }
247 }
248
249 /// Create a TOPP3 builder with required fields.
250 ///
251 /// Defaults:
252 /// - `num_stationary_max = (1, 1)`
253 /// - `a_linearization_floor = 1E-10`
254 pub fn with_constraint(
255 constraints: &'a mut Constraints,
256 idx_s_start: usize,
257 a_linearization: &'a [f64],
258 a_boundary: (f64, f64),
259 b_boundary: (f64, f64),
260 ) -> Self {
261 Self {
262 constraints,
263 idx_s_start,
264 a_linearization,
265 a_boundary,
266 b_boundary,
267 num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
268 a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
269 }
270 }
271
272 /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
273 ///
274 /// See module-level **Stationary-boundary modeling note** for guidance.
275 #[inline]
276 pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
277 self.num_stationary_max = (num_stationary_max, num_stationary_max);
278 self
279 }
280
281 /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
282 ///
283 /// See module-level **Stationary-boundary modeling note** for guidance.
284 #[inline]
285 pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
286 self.num_stationary_max = num_stationary_max;
287 self
288 }
289
290 /// Set denominator floor used in third-order linearization near `a = 0`.
291 ///
292 /// [`build_with_linearization`](Self::build_with_linearization) applies
293 /// `1.0 / max(a_linearization[k], floor).sqrt()` when converting nonlinear
294 /// jerk rows into affine rows.
295 #[inline]
296 pub fn with_a_linearization_floor(mut self, floor: f64) -> Self {
297 self.a_linearization_floor = floor;
298 self
299 }
300
301 /// Build a TOPP3 problem and linearize third-order constraints in one step.
302 ///
303 /// This validates boundaries/interval/floor first, then writes linearized jerk buffers
304 /// inside [`Constraints`](crate::constraints::Constraints). The generated rows
305 /// can be inspected with
306 /// [`Constraints::get_jerk_linear_constraints`](crate::constraints::Constraints::get_jerk_linear_constraints).
307 ///
308 /// # Rebuilding rule
309 /// If a later solver result is used as a new
310 /// [`a_linearization`](Topp3ProblemBuilder::a_linearization), create a new
311 /// builder and call [`build_with_linearization`](Topp3ProblemBuilder::build_with_linearization)
312 /// again before solving.
313 pub fn build_with_linearization(self) -> Result<Topp3Problem<'a>, CoppError> {
314 check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
315 if self.a_linearization.is_empty() {
316 return Err(CoppError::InvalidInput(
317 "Topp3ProblemBuilder::build_with_linearization".into(),
318 "a_linearization cannot be empty".into(),
319 ));
320 }
321 let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
322 check_s_interval_valid(
323 "Topp3ProblemBuilder::build_with_linearization",
324 self.idx_s_start,
325 idx_s_final,
326 )?;
327 if self.a_linearization_floor <= 0.0 {
328 return Err(CoppError::InvalidInput(
329 "Topp3ProblemBuilder::build_with_linearization".into(),
330 format!(
331 "a_linearization_floor must be positive, got {}",
332 self.a_linearization_floor
333 ),
334 ));
335 }
336
337 self.constraints
338 .linearize_constraint_3order_with_floor(
339 self.a_linearization,
340 self.idx_s_start,
341 self.a_linearization_floor,
342 )
343 .map_err(|e| {
344 CoppError::InvalidInput(
345 "Topp3ProblemBuilder::build_with_linearization".into(),
346 format!("linearize_constraint_3order failed: {e}"),
347 )
348 })?;
349
350 let num_stationary = determine_num_stationary_pair(
351 self.a_boundary,
352 self.b_boundary,
353 self.num_stationary_max,
354 );
355
356 Ok(Topp3Problem {
357 constraints: &*self.constraints,
358 idx_s_start: self.idx_s_start,
359 a_linearization: self.a_linearization,
360 a_boundary: self.a_boundary,
361 b_boundary: self.b_boundary,
362 num_stationary,
363 })
364 }
365}
366
367/// The problem of COPP3.
368/// # Arguments
369/// * `robot` - A robot with torque implemented, which defines the constraints and dynamic of the problem.
370/// * `objectives` - Objectives for COPP3 optimization.
371/// * `idx_s_start` - The starting index along the path (reached).
372/// * `a_linearization` - Linearization reference profile for third-order constraints.
373/// * `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.
374/// * `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.
375/// * `num_stationary=(start,end)` - Effective stationary intervals derived in `build_with_linearization()`.
376pub struct Copp3Problem<'a, M: RobotTorque> {
377 pub(crate) robot: &'a mut Robot<M>,
378 pub(crate) objectives: &'a [CoppObjective<'a>],
379 pub(crate) idx_s_start: usize,
380 pub(crate) a_linearization: &'a [f64],
381 pub(crate) a_boundary: (f64, f64),
382 pub(crate) b_boundary: (f64, f64),
383 /// Effective stationary intervals at (start, end), derived in `build_with_linearization()` from
384 /// boundary conditions and `num_stationary_max`.
385 pub(crate) num_stationary: (usize, usize),
386}
387
388/// Builder for [`Copp3Problem`](crate::solver::copp3_socp::Copp3Problem).
389///
390/// # Why mutable?
391/// COPP3 uses the same third-order linearization cache as TOPP3. The builder
392/// takes [`&mut Robot<M>`](crate::robot::RobotTorque) because
393/// [`build_with_linearization`](Copp3ProblemBuilder::build_with_linearization) converts nonconvex
394/// jerk rows into affine rows around [`a_linearization`](Copp3ProblemBuilder::a_linearization) and stores those rows in
395/// [`robot.constraints`](crate::robot::Robot::constraints).
396/// Rebuild the problem whenever the linearization profile changes.
397///
398/// # Example
399/// The example below builds a COPP3 problem with time and thermal objectives, including the required third-order linearization profile.
400///
401/// ```rust
402/// # fn main() -> Result<(), copp::diag::CoppError> {
403/// use copp::robot::Robot;
404/// use copp::solver::copp3_socp::{Copp3ProblemBuilder, CoppObjective};
405///
406/// let mut robot = Robot::with_capacity(2usize, 3);
407/// let s = [0.0, 0.5, 1.0];
408/// robot.with_s(s.as_slice())?;
409///
410/// let a_linearization = [0.0, 0.25, 0.0];
411/// let normalize = [1.0, 1.0];
412/// let objectives = [
413/// CoppObjective::Time(1.0),
414/// CoppObjective::ThermalEnergy(0.1, &normalize),
415/// ];
416///
417/// let _problem = Copp3ProblemBuilder::new(
418/// &mut robot,
419/// &objectives,
420/// 0,
421/// &a_linearization,
422/// (0.0, 0.0),
423/// (0.0, 0.0),
424/// )
425/// .build_with_linearization()?;
426/// # Ok(())
427/// # }
428/// ```
429pub struct Copp3ProblemBuilder<'a, M: RobotTorque> {
430 /// A robot with torque implemented, which defines the constraints and dynamic of the problem.
431 pub robot: &'a mut Robot<M>,
432 /// Objectives for COPP3 optimization.
433 pub objectives: &'a [CoppObjective<'a>],
434 /// The starting index along the path (reached).
435 pub idx_s_start: usize,
436 /// Linearization reference profile for third-order constraints.
437 pub a_linearization: &'a [f64],
438 /// `a_boundary=(a_start,a_final)` - The initial and final acceleration at the start and end of the path.
439 pub a_boundary: (f64, f64),
440 /// `b_boundary=(b_start,b_final)` - The initial and final boundary conditions for `b`.
441 pub b_boundary: (f64, f64),
442 /// User-input upper bound of stationary intervals at (start, end).
443 pub num_stationary_max: (usize, usize),
444 /// Denominator floor for stable evaluation of `1/sqrt(a_linearization)` near `a=0`.
445 ///
446 /// Effective usage in linearization is:
447 /// $$
448 /// \frac{1}{\sqrt{\max(a_{lin}, a_{floor})}}.
449 /// $$
450 /// Discrete code form:
451 /// `1.0 / max(a_linearization, a_linearization_floor).sqrt()`.
452 ///
453 /// More details are available in the [`Topp3Problem`](crate::solver::topp3_lp::Topp3Problem) documentation.
454 pub a_linearization_floor: f64,
455}
456
457impl<'a, M: RobotTorque> Copp3ProblemBuilder<'a, M> {
458 /// Create a COPP3 builder with required fields.
459 ///
460 /// Defaults:
461 /// - `num_stationary_max = (1, 1)`
462 /// - `a_linearization_floor = 1E-10`
463 pub fn new(
464 robot: &'a mut Robot<M>,
465 objectives: &'a [CoppObjective<'a>],
466 idx_s_start: usize,
467 a_linearization: &'a [f64],
468 a_boundary: (f64, f64),
469 b_boundary: (f64, f64),
470 ) -> Self {
471 Self {
472 robot,
473 objectives,
474 idx_s_start,
475 a_linearization,
476 a_boundary,
477 b_boundary,
478 num_stationary_max: DEFAULT_NUM_STATIONARY_MAX,
479 a_linearization_floor: DEFAULT_A_LINEARIZATION_FLOOR,
480 }
481 }
482
483 /// Set symmetric stationary upper bound: `num_stationary_max=(n,n)`.
484 ///
485 /// See module-level **Stationary-boundary modeling note** for guidance.
486 #[inline]
487 pub fn with_num_stationary_max(mut self, num_stationary_max: usize) -> Self {
488 self.num_stationary_max = (num_stationary_max, num_stationary_max);
489 self
490 }
491
492 /// Set asymmetric stationary upper bound: `num_stationary_max=(start,end)`.
493 ///
494 /// See module-level **Stationary-boundary modeling note** for guidance.
495 #[inline]
496 pub fn with_num_stationary_max_pair(mut self, num_stationary_max: (usize, usize)) -> Self {
497 self.num_stationary_max = num_stationary_max;
498 self
499 }
500
501 /// Set denominator floor used in third-order linearization near `a = 0`.
502 ///
503 /// [`build_with_linearization`](Self::build_with_linearization) applies
504 /// `1.0 / max(a_linearization[k], floor).sqrt()` when converting nonlinear
505 /// jerk rows into affine rows.
506 #[inline]
507 pub fn with_a_linearization_floor(mut self, floor: f64) -> Self {
508 self.a_linearization_floor = floor;
509 self
510 }
511
512 /// Build a validated COPP3 problem and linearize third-order constraints in one step.
513 ///
514 /// This validates boundaries/interval/floor first, then writes linearized jerk buffers
515 /// inside [`Constraints`](crate::constraints::Constraints). The generated rows
516 /// can be inspected with
517 /// [`Constraints::get_jerk_linear_constraints`](crate::constraints::Constraints::get_jerk_linear_constraints).
518 ///
519 /// # Rebuilding rule
520 /// If a later solver result is used as a new [`a_linearization`](Copp3ProblemBuilder::a_linearization), create a new
521 /// builder and call [`build_with_linearization`](Copp3ProblemBuilder::build_with_linearization) again before solving.
522 pub fn build_with_linearization(self) -> Result<Copp3Problem<'a, M>, CoppError> {
523 check_boundary_state_copp3_valid(self.a_boundary, self.b_boundary)?;
524 if self.a_linearization.is_empty() {
525 return Err(CoppError::InvalidInput(
526 "Copp3ProblemBuilder::build_with_linearization".into(),
527 "a_linearization cannot be empty".into(),
528 ));
529 }
530 let idx_s_final = self.idx_s_start + self.a_linearization.len() - 1;
531 check_s_interval_valid(
532 "Copp3ProblemBuilder::build_with_linearization",
533 self.idx_s_start,
534 idx_s_final,
535 )?;
536 self.robot
537 .constraints
538 .check_s_in_bounds(self.idx_s_start, self.a_linearization.len())?;
539 if self.a_linearization_floor <= 0.0 {
540 return Err(CoppError::InvalidInput(
541 "Copp3ProblemBuilder::build_with_linearization".into(),
542 format!(
543 "a_linearization_floor must be positive, got {}",
544 self.a_linearization_floor
545 ),
546 ));
547 }
548
549 self.robot
550 .constraints
551 .linearize_constraint_3order_with_floor(
552 self.a_linearization,
553 self.idx_s_start,
554 self.a_linearization_floor,
555 )
556 .map_err(|e| {
557 CoppError::InvalidInput(
558 "Copp3ProblemBuilder::build_with_linearization".into(),
559 format!("linearize_constraint_3order failed: {e}"),
560 )
561 })?;
562
563 let num_stationary = determine_num_stationary_pair(
564 self.a_boundary,
565 self.b_boundary,
566 self.num_stationary_max,
567 );
568
569 Ok(Copp3Problem {
570 robot: self.robot,
571 objectives: self.objectives,
572 idx_s_start: self.idx_s_start,
573 a_linearization: self.a_linearization,
574 a_boundary: self.a_boundary,
575 b_boundary: self.b_boundary,
576 num_stationary,
577 })
578 }
579}
580
581impl<'a, M: RobotTorque> Copp3Problem<'a, M> {
582 /// Update linearization profile.
583 #[inline]
584 pub fn set_a_linearization(&mut self, a_linearization: &'a [f64]) {
585 self.a_linearization = a_linearization;
586 }
587
588 /// Backward-compatible alias for `set_a_linearization`.
589 #[inline]
590 pub fn set_a_linear(&mut self, a_linearization: &'a [f64]) {
591 self.set_a_linearization(a_linearization);
592 }
593
594 /// Update objective list.
595 #[inline]
596 pub fn set_objective(&mut self, objective: &'a [CoppObjective<'a>]) {
597 self.objectives = objective;
598 }
599
600 /// Convert to the TOPP3 view that shares interval/boundary/linearization fields.
601 pub fn as_topp3_problem(&self) -> Topp3Problem<'_> {
602 Topp3Problem {
603 constraints: &self.robot.constraints,
604 idx_s_start: self.idx_s_start,
605 a_linearization: self.a_linearization,
606 a_boundary: self.a_boundary,
607 b_boundary: self.b_boundary,
608 num_stationary: self.num_stationary,
609 }
610 }
611}
612
613/// Get the weight of `a` for value function.
614/// Time loss = \sum_{k=0}^n weight_a[k] / sqrt(a[k]) \approx Time
615pub(crate) fn get_weight_a_topp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
616 let n = s.len() - 1;
617 let mut weight_a = vec![0.0; s.len()];
618 if num_stationary.0 > 0 {
619 weight_a[num_stationary.0] =
620 0.5 * (5.0 * s[num_stationary.0] + s[num_stationary.0 + 1] - 6.0 * s[0]);
621 }
622 if num_stationary.1 > 0 {
623 weight_a[n - num_stationary.1] =
624 0.5 * (6.0 * s[n] - 5.0 * s[n - num_stationary.1] - s[n - num_stationary.1 - 1]);
625 }
626 weight_a
627 .iter_mut()
628 .skip(1)
629 .zip(s.windows(3))
630 .skip(num_stationary.0)
631 .take(n - num_stationary.0 - num_stationary.1 - 1)
632 .for_each(|(w_a, s_slice)| {
633 *w_a = 0.5 * (s_slice[2] - s_slice[0]);
634 });
635
636 weight_a
637}
638
639/// Get the weight of `a` for value function.
640/// Loss = weight[0] * loss_average_left / sqrt(a[num_stationary.0]) + weight[n] * loss_average_right / sqrt(a[n - num_stationary.1]) + \sum_{k=num_stationary.0}^{n-num_stationary.1-1} weight_a[k] * loss[k] / sqrt(a[k])
641pub(crate) fn get_weight_a_copp3(s: &[f64], num_stationary: (usize, usize)) -> Vec<f64> {
642 let n = s.len() - 1;
643 let mut weight_a = vec![0.0; s.len()];
644 if num_stationary.0 > 0 {
645 let s_n1 = s[num_stationary.0];
646 weight_a[num_stationary.0] = 0.5 * (s[num_stationary.0 + 1] - s_n1);
647 weight_a[0] = 3.0 * (s_n1 - s[0]);
648 }
649 if num_stationary.1 > 0 {
650 let s_n2 = s[n - num_stationary.1];
651 weight_a[n - num_stationary.1] = 0.5 * (s_n2 - s[n - num_stationary.1 - 1]);
652 weight_a[n] = 3.0 * (s[n] - s_n2);
653 }
654 weight_a
655 .iter_mut()
656 .skip(1)
657 .zip(s.windows(3))
658 .skip(num_stationary.0)
659 .take(n - num_stationary.0 - num_stationary.1 - 1)
660 .for_each(|(w_a, s_slice)| {
661 *w_a = 0.5 * (s_slice[2] - s_slice[0]);
662 });
663
664 weight_a
665}
666
667/// Get the `a` and `b` profiles for stationary intervals.
668pub(crate) fn set_ab_stationary_topp3<const START: bool>(
669 s: &[f64],
670 a: &mut [f64],
671 b: &mut [f64],
672 a_stationary: f64,
673 num_stationary: usize,
674) {
675 if num_stationary == 0 {
676 return;
677 }
678 if START {
679 let s_start = s[0];
680 let ds_start = s[num_stationary] - s_start;
681 *a.first_mut().unwrap() = 0.0;
682 *b.first_mut().unwrap() = 0.0;
683 a[num_stationary] = a_stationary;
684 b[num_stationary] = a_stationary / (1.5 * ds_start);
685 if num_stationary > 1 {
686 for (a_k, b_k, &s_k) in izip!(a.iter_mut(), b.iter_mut(), s.iter())
687 .skip(1)
688 .take(num_stationary - 1)
689 {
690 let dsk_start = s_k - s_start;
691 let mut alpha = dsk_start / ds_start;
692 alpha *= alpha.cbrt();
693 *a_k = a_stationary * alpha;
694 *b_k = *a_k / (1.5 * dsk_start);
695 }
696 }
697 } else {
698 let &s_final = s.last().unwrap();
699 let n = s.len() - 1;
700 let ds_final = s[n - num_stationary] - s_final;
701 *a.last_mut().unwrap() = 0.0;
702 *b.last_mut().unwrap() = 0.0;
703 a[a.len() - 1 - num_stationary] = a_stationary;
704 b[b.len() - 1 - num_stationary] = a_stationary / (1.5 * ds_final);
705 if num_stationary > 1 {
706 for (a_k, b_k, &s_k) in izip!(a.iter_mut().rev(), b.iter_mut().rev(), s.iter().rev())
707 .skip(1)
708 .take(num_stationary - 1)
709 {
710 let dsk_final = s_k - s_final;
711 let mut alpha = dsk_final / ds_final;
712 alpha *= alpha.cbrt();
713 *a_k = a_stationary * alpha;
714 *b_k = *a_k / (1.5 * dsk_final);
715 }
716 }
717 }
718}
719
720#[cfg(test)]
721mod tests {
722 use super::determine_num_stationary_pair;
723
724 #[test]
725 fn test_determine_num_stationary_pair_respects_boundary_state() {
726 let pair = determine_num_stationary_pair((0.0, 0.0), (0.0, 0.0), (2, 3));
727 assert_eq!(pair, (2, 3));
728
729 let pair = determine_num_stationary_pair((1.0, 0.0), (0.0, 0.0), (2, 3));
730 assert_eq!(pair, (0, 3));
731
732 let pair = determine_num_stationary_pair((0.0, 1.0), (0.0, 1.0), (2, 3));
733 assert_eq!(pair, (2, 0));
734 }
735}